/*
 * renegadesvs_driver.cpp
 *
 *  Created on: Oct 11, 2011
 *      Author: dlobato
 */

#include "renegadesvs_driver.h"
#include <iostream>

RenegadeSVS_Driver::RenegadeSVS_Driver(ros::NodeHandle nh):
	SurveyorSVS_Driver(nh),
	cmdvel_sub(nh.subscribe("cmd_vel", 1, &RenegadeSVS_Driver::cmdvel_cb, this)),
	cmdptz_sub(nh.subscribe("ptz_control", 1, &RenegadeSVS_Driver::cmdptz_cb, this)) {

	//read params from this->priv_nh
	this->priv_nh.param("max_tv",this->max_tv,1.0);
	this->priv_nh.param("max_rv",this->max_rv,1.0);
	this->priv_nh.param("max_pan",this->max_pan,0.7);
	this->priv_nh.param("max_tilt",this->max_tilt,0.7);

	ROS_INFO_STREAM("Reseting motors and pan-tilt...");
	//max values
	if (svs_set_servo1_max_values(&this->svs_dev->modules[SVS_MASTER],this->max_tv, this->max_rv) != 0)
		ROS_ERROR("Error setting servo1 max values");

	vel_time = ros::Time::now();
	vel_dirty = true;
	cmdvel.linear.x = 0.0;
	cmdvel.angular.z = 0.0;

	if (svs_set_servo2_max_values(&this->svs_dev->modules[SVS_MASTER],this->max_pan, this->max_tilt) != 0)
		ROS_ERROR("Error setting servo2 max values");
	ptz_time = vel_time;
	ptz_dirty = true;
	cmdptz.pan = 0.0;
	cmdptz.tilt = 0.0;
}

RenegadeSVS_Driver::~RenegadeSVS_Driver(){}

void RenegadeSVS_Driver::poll(){
	SurveyorSVS_Driver::poll();
	check_and_set_vel();
	check_and_set_ptz();
}

void RenegadeSVS_Driver::cmdvel_cb(const geometry_msgs::TwistConstPtr &msg) {
	//borrowed from p2os/p2os_driver/src/p2os.cc
	if (fabs(msg->linear.x - this->cmdvel.linear.x) > 0.01 || fabs(msg->angular.z - this->cmdvel.angular.z) > 0.01) {
		this->vel_time = ros::Time::now();
		ROS_DEBUG( "new speed: [%0.2f,%0.2f](%0.3f)", msg->linear.x*1e3, msg->angular.z, this->vel_time.toSec() );
		this->vel_dirty = true;
		this->cmdvel = *msg;
	} else {
		ros::Duration veldur = ros::Time::now() - this->vel_time;
		if (veldur.toSec() > 2.0 && ((fabs(this->cmdvel.linear.x) > 0.01) || (fabs(this->cmdvel.angular.z) > 0.01))) {
			ROS_DEBUG( "maintaining old speed: %0.3f|%0.3f", this->vel_time.toSec(), ros::Time::now().toSec() );
			this->vel_dirty = true;
			this->vel_time = ros::Time::now();
		}
	}
}

void RenegadeSVS_Driver::check_and_set_vel(){
	if( !this->vel_dirty ) return;

	ROS_DEBUG( "setting vel: [%0.2f,%0.2f]",this->cmdvel.linear.x,this->cmdvel.angular.z);
	this->vel_dirty = false;

	if (svs_set_servo1_values(&this->svs_dev->modules[SVS_MASTER], this->cmdvel.linear.x, this->cmdvel.angular.z) != 0)//FIXME: units?
		ROS_ERROR("Error setting vel cmd");
}

void RenegadeSVS_Driver::cmdptz_cb(const surveyorsvs_driver::PTZStateConstPtr &msg){
	ROS_DEBUG_STREAM("cmdptz received:" << msg);
}

void RenegadeSVS_Driver::check_and_set_ptz(){
	if( !this->ptz_dirty ) return;

	ROS_DEBUG( "setting ptz: [%d,%d]",this->cmdptz.pan,this->cmdptz.tilt);
	this->ptz_dirty = false;
	//FIXME
	//svs_set_servo2_values(&this->svs_dev->modules[SVS_MASTER], this->cmdptz.pan,this->cmdptz.tilt);//units??
}
